#include <stdio.h>
#include <stdlib.h>
#include <math.h>

inline double nplusp(double *n,double *p)
{
	return p[0]*n[0]+p[1]*n[1]+p[2]*n[2];
}//也就是向量n乘以向量p，用于test函数

void Initn(double *x,double *n)
{
	double t[6];
	t[0]=x[0]-x[3];
	t[1]=x[1]-x[4];	
	t[2]=x[2]-x[5];	
	t[3]=x[0]-x[6];	
	t[4]=x[1]-x[7];	
	t[5]=x[2]-x[8];	
	n[0]=t[1]*t[5]-t[2]*t[4];
	n[1]=t[2]*t[3]-t[0]*t[5];
	n[2]=t[0]*t[4]-t[1]*t[3];	
}//生成 以x为坐标的平面的 法向量 运用的是叉乘的原理，主要用于test函数，同时进行x平面的法向量初始化。

int test(double * s,double * l,double * xn,double * x)
{
	//p为s＋0.01*l，也就是说，s与x形成了四面体，验证p是否在这个四面体内，方法是分别验证p和一个顶点在此顶点所对平面同侧。
	double p[3],t[9],r[3],n[3],temp;
	p[0]=s[0]+0.01*l[0];
	p[1]=s[1]+0.01*l[1];	
	p[2]=s[2]+0.01*l[2];
	temp=nplusp(xn,x);
	//设向量l为平面x的一点和非平面一点之间向量， (nplusp(xn,p)-temp)表示的此向量与平面法向量的乘积，两个乘积如果符号相同，所对应的非平面点同侧
	if((nplusp(xn,p)-temp)*(nplusp(xn,s)-temp)>=0)
	{
		t[0]=s[0];
		t[1]=s[1];
		t[2]=s[2];
		t[3]=x[3];
		t[4]=x[4];
		t[5]=x[5];
		t[6]=x[6];
		t[7]=x[7];
		t[8]=x[8];
		Initn(t,n);
		r[0]=x[3];r[1]=x[4];r[2]=x[5];
		temp=nplusp(n,r);		
		if((nplusp(n,x)-temp)*(nplusp(n,p)-temp)>=0)
		{
			t[0]=x[0];
			t[1]=x[1];
			t[2]=x[2];
			t[3]=s[0];
			t[4]=s[1];
			t[5]=s[2];
			r[0]=x[3];
			r[1]=x[4];
			r[2]=x[5];
			Initn(t,n);
			temp=nplusp(n,x);	
			if((nplusp(n,r)-temp)*(nplusp(n,p)-temp)>=0)
			{
				t[3]=x[3];
				t[4]=x[4];
				t[5]=x[5];
				t[6]=s[0];
				t[7]=s[1];
				t[8]=s[2];
				r[0]=x[6];
				r[1]=x[7];
				r[2]=x[8];
				Initn(t,n);
				temp=nplusp(n,x);	
				if((nplusp(n,r)-temp)*(nplusp(n,p)-temp)>=0)
					return 1;
			}	
		}
	}
	return 0;
}//对平面x是否被射线l穿越进行测试，穿越返回1，不穿越返回0。

double dis(double * s,double * l, double * n,double *x)//计算s点延l方向对x的距离
{	
	double r0=s[0]-x[0];
	double r1=s[1]-x[1];
	double r2=s[2]-x[2];
	double t1=r0*r0+r1*r1+r2*r2;
	double d1=sqrt(t1);//s点到x[0]点的距离
	double t2=n[0]*n[0]+n[1]*n[1]+n[2]*n[2];
	double d2=sqrt(t2);//平面法向量的长度
	double t3=l[0]*l[0]+l[1]*l[1]+l[2]*l[2];
	double d3=sqrt(t3);//l方向向量的长度
	double cos1=(n[0]*r0+n[1]*r1+n[2]*r2)/(d1*d2); 
	if(cos1<0) cos1=-cos1;//平面方向向量与x[0]-s向量之间的夹角的余弦，可以求得s到平面的距离
	double dn=d1*cos1;//s到平面的距离
	double cos2=(n[0]*l[0]+n[1]*l[1]+n[2]*l[2])/(d2*d3);//平面方向向量与p-s向量之间的夹角的余弦，可通过s到平面的距离求得ps距离
	if(cos2<0) cos2=-cos2;
	return dn/cos2;
}

void point(double *s,double *l,double dp,double *p)//生成 所对应的交点
{
	double t3=l[0]*l[0]+l[1]*l[1]+l[2]*l[2];
	double d3=sqrt(t3);
	p[0]=s[0]+l[0]*dp/d3;
	p[1]=s[1]+l[1]*dp/d3;
	p[2]=s[2]+l[2]*dp/d3;
	return;	
}
